10. Occupancy Grid Mapping Algorithm

Occupancy Grid Mapping Algorithm

Now that you've learned the Occupancy Grid Mapping algorithm, you will get a chance to code it in C++!

In this quiz, a robot equipped with eight sonar rangefinder sensors circulates in an environment to map it. This robot is provided with its exact poses at each timestamp. The code structure is as follows:

Data Files

  1. measurement.txt : The measurements from the sonar rangefinder sensors attached to the robot at each time stamp recorded over a period of 413 seconds. (timestamp, measurement 1:8).
  2. poses.txt : The exact robot poses at each timestamp recorded over a period of 413 seconds. (timestamp, x, y, ϴ).

Global Functions

  1. inverseSensorModel(): You'll code this function as part of your second quiz after learning the inverse sensor model for sonar rangefinder sensors.
  2. occupancyGridMapping(): You'll code this function as part of your first quiz.

Main Function

  1. File Scan: Scanning both the measurement and poses files to retrieve the values. At each time stamp, the values are passed to the occupancy grid mapping function.
  2. Display Map: After processing all the measurements and poses, the map is displayed.

Now, code the occupancyGridMapping() function:

Task Description:

Here are the steps that you need to follow in order to code the occupancyGridMapping() function:

Task List:

Task Feedback:

Great Job!

A cell would usually fall under the perceptual field of the measurements if the distance between the cell centroid and the robot pose is smaller or equal than the maximum measurements Zmax

Start Quiz:

#include <iostream>
#include <math.h>
#include <vector>
using namespace std;

// Sensor characteristic: Min and Max ranges of the beams
double Zmax = 5000, Zmin = 170;
// Defining free cells(lfree), occupied cells(locc), unknown cells(l0) log odds values
double l0 = 0, locc = 0.4, lfree = -0.4;
// Grid dimensions
double gridWidth = 100, gridHeight = 100;
// Map dimensions
double mapWidth = 30000, mapHeight = 15000;
// Robot size with respect to the map 
double robotXOffset = mapWidth / 5, robotYOffset = mapHeight / 3;
// Defining an l vector to store the log odds values of each cell
vector< vector<double> > l(mapWidth/gridWidth, vector<double>(mapHeight/gridHeight));

double inverseSensorModel(double x, double y, double theta, double xi, double yi, double sensorData[])
{
    // You will be coding this section in the upcoming concept! 
    return 0.4;
}

void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[])
{
    //1 - TODO: Generate a grid (size 300x150) and then loop through all the cells
            //2- TODO: Compute the center of mass of each cell xi and yi 
            //double xi = x * gridWidth + gridWidth / 2 - robotXOffset;
            //double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset;
            //3- TODO: Check if each cell falls under the perceptual field of the measurements
    
    
    
    
    
    
}

int main()
{
    double timeStamp;
    double measurementData[8];
    double robotX, robotY, robotTheta;

    FILE* posesFile = fopen("poses.txt", "r");
    FILE* measurementFile = fopen("measurement.txt", "r");

    // Scanning the files and retrieving measurement and poses at each timestamp
    while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) {
        fscanf(measurementFile, "%lf", &timeStamp);
        for (int i = 0; i < 8; i++) {
            fscanf(measurementFile, "%lf", &measurementData[i]);
        }
        occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData);
    }
    
    // Displaying the map
    for (int x = 0; x < mapWidth / gridWidth; x++) {
        for (int y = 0; y < mapHeight / gridHeight; y++) {
            cout << l[x][y] << " ";
        }
    }
    
    return 0;
}
1686487 5110 5110 2320 2360 5110 5110 2160 1190
1686915 5110 5110 2320 2360 5110 5110 2160 1190
1687132 5110 5110 2320 2360 5110 5110 2160 1190
1687555 5110 5110 2320 2360 5110 5110 2160 1190
1687770 5110 5110 2320 2360 5110 5110 2160 1190
1688194 5110 5110 2320 2360 5110 5110 2170 1190
1688411 5110 5110 2320 2360 5110 5110 2160 1180
1688627 5110 5110 1040 2360 5110 5110 2160 1180
1688843 5110 5110 1040 2360 5110 5110 2170 1190
1689054 5110 5110 2320 2370 5110 5110 2170 1190
1689273 5110 5110 2320 2360 5110 5110 2160 1180
1689479 5110 5110 2320 2360 5110 5110 2170 1190
1689698 5110 5110 2320 2370 5110 5110 2170 1190
1689906 5110 5110 2320 2370 5110 5110 2160 1180
1690116 5110 5110 2320 2360 5110 5110 2160 1180
1690338 5110 5110 2320 2360 5110 5110 2160 1190
1690549 5110 5110 2320 2360 5110 5110 2160 1190
1690759 5110 5110 2320 2360 5110 5110 2160 1190
1690970 5110 5110 2320 2350 5110 5110 2160 1190
1691192 5110 5110 2320 2350 5110 5110 2160 1190
1691400 5110 5110 2320 2360 5110 5110 2160 1190
1691822 5110 5110 2320 2350 5110 5110 2160 1190
1692039 5110 5110 2320 2350 5110 5110 2160 1190
1692258 5110 5110 2320 400 5110 5110 2160 1190
1692470 5110 5110 2320 2360 5110 5110 2170 1190
1692681 5110 5110 2320 2360 5110 5110 2160 1180
1692900 5110 5110 2320 2350 5110 5110 2160 1180
1693105 5110 5110 2320 2350 5110 5110 2170 1190
1693319 5110 5110 2320 2360 5110 5110 2160 1190
1693530 5110 5110 2320 2360 5110 5110 2160 1180
1693751 5110 5110 2320 2360 5110 5110 2170 1180
1693959 5110 5110 2320 2360 5110 5110 2170 1190
1694178 5110 5110 2320 2360 5110 5110 2160 1190
1694387 5110 5110 2320 2360 5110 5110 2160 1190
1694811 5110 5110 2320 2350 5110 5110 2160 1190
1695028 5110 5110 2320 2360 5110 5110 2160 1190
1695243 5110 5110 2320 2360 5110 5110 2160 1190
1695451 5110 5110 2320 2360 5110 5110 2160 1190
1695665 5110 5110 2320 2360 5110 5110 2170 1190
1695888 5110 5110 2320 2350 5110 5110 2170 1190
1696098 5110 5110 2320 2350 5110 5110 2160 1180
1696314 5110 5110 2320 2360 5110 5110 2160 1180
1696524 5110 5110 2320 2360 5110 5110 2160 1190
1696740 5110 5110 2320 2360 5110 5110 2160 1190
1696952 5110 5110 2320 2360 5110 5110 2160 1180
1697168 5110 5110 2320 2360 5110 5110 2160 1180
1697377 5110 5110 2320 2360 5110 5110 2170 1190
1697590 5110 5110 2310 2360 5110 5110 2170 1190
1697802 5110 5110 2310 2360 5110 5110 2150 1180
1698013 5110 5110 2290 2310 5060 5110 2150 1180
1698233 5110 5110 2290 2310 5060 5110 2100 1190
1698442 5110 2240 2210 2240 4970 5110 2100 1190
1698652 5110 2240 2210 2240 4970 4950 2000 1260
1698868 860 2120 2100 4260 4860 4830 1910 1260
1699087 860 2010 2000 4140 5110 4830 1910 1260
1699293 870 2010 2000 4140 5110 4700 1830 1260
1699515 870 1900 1890 4020 5110 4700 1830 1260
1699721 860 1900 1890 4020 5110 4590 1740 1260
1699939 860 1800 1810 3910 5110 4480 1740 1260
1700150 870 1700 1810 3910 5110 4480 5110 1220
1700362 870 1700 2700 3800 5110 4370 5110 1220
1700578 870 1600 2700 3800 5110 4370 5080 1180
1700788 870 1600 2600 3680 5110 4250 5080 1180
1701000 860 1500 2600 3680 5110 4250 5110 1180
1701218 860 1500 2480 3570 5110 4140 5110 1180
1701435 830 1410 2480 3570 5110 4140 5110 1180
1701645 830 1410 2380 3450 5110 4030 5110 1180
1701857 920 1320 2380 3450 5110 4030 5110 1260
1702070 920 1320 2270 2290 5110 3900 5110 1260
1702289 920 5010 2150 2170 5110 3790 5110 1270
1702500 920 2070 2050 2170 5110 3790 5110 1270
1702713 920 2070 2050 2170 5110 2270 2390 1270
1702928 920 4800 1960 2170 5110 2270 2390 1270
1703144 920 4800 1960 1990 5110 3620 1670 1270
1703349 910 4720 1880 1990 5110 3620 1670 1220
1703566 910 4720 1880 1910 5110 3470 5110 1220
1703775 900 1810 1880 1910 5110 3470 5110 1210
1703990 900 1810 1880 2800 5110 3410 5110 1210
1704210 890 1700 1680 2800 5110 3410 5110 1210
1704423 890 1700 1680 2800 5110 3340 5110 1220
1704637 880 1590 1580 2600 5110 3340 5110 1220
1704845 880 1590 1580 2600 5110 3210 3220 1220
1705065 860 1480 2480 3550 5110 3210 3220 1220
1705272 860 1480 2480 3550 5110 3130 3110 1230
1705488 820 1380 2370 3450 5110 3020 3000 1230
1705695 840 1300 2260 3320 5110 3020 3000 1230
1705912 840 1300 2260 3320 5110 2900 2890 1240
1706122 860 1200 2150 3220 5110 2900 2890 1240
1706339 860 1200 2150 3220 5110 2820 2780 1260
1706549 860 1140 2030 2070 5110 2820 2780 1260
1706767 850 1140 2030 2070 5110 2820 2680 1250
1706977 850 5110 1930 1960 5110 2820 2680 1250
1707189 850 5110 1930 1960 5110 5110 2570 1260
1707409 850 1850 1820 2870 5110 5110 2570 1260
1707616 840 1850 1820 2870 5110 5110 2470 1260
1707834 840 1740 1730 2780 5110 5110 2470 1260
1708047 840 1740 1730 2780 5110 5110 2390 1260
1708261 840 1640 1630 2680 5110 5110 2390 1260
1708478 840 1550 1630 2580 5110 5110 2290 1260
1708688 790 1440 1630 2580 5110 5110 2200 1270
1708902 790 1440 2440 3120 5110 5010 2200 1270
1709118 780 1340 2440 3120 5110 5010 2090 1270
1709329 780 1340 2330 3000 5110 4900 2090 1270
1709540 820 1250 2330 3000 5110 4900 2020 1270
1709759 820 1250 2230 3610 5110 4780 2020 1270
1709968 830 1170 2230 3610 5110 4780 1950 1280
1710181 830 1170 2120 3490 5110 4670 1950 1280
1710391 820 5110 2120 3490 5110 4670 1900 1280
1710611 820 5110 2010 2030 5110 4550 1900 1280
1710820 820 5110 2010 2030 5110 4550 5110 1280
1711032 820 5110 1900 1920 5110 4450 5110 1280
1711247 820 1850 1820 1920 5110 4450 5110 1280
1711464 820 1850 1820 3190 5110 4340 5110 1280
1711882 810 1630 1620 3110 5110 4230 5110 1280
1712100 810 1630 1620 3000 5110 4120 5110 1280
1712317 810 1520 1620 3000 5110 4120 5110 1330
1712525 810 1520 1620 2910 5110 4020 5110 1330
1712739 770 1410 2010 2910 5110 4020 5110 1370
1712952 770 1410 2010 2790 5110 3910 5110 1370
1713170 760 1310 1360 2690 5110 3910 5110 1380
1713385 760 1310 1360 2690 5110 3800 5110 1380
1713599 810 1230 1800 5110 5110 3800 5110 1380
1713813 810 1230 1800 5110 5110 3690 5110 1360
1714021 800 1140 1710 5110 5110 3690 5110 1360
1714235 800 1140 1710 5110 5110 3590 5110 1320
1714452 800 1080 1610 5110 5110 3590 5110 1320
1714657 800 1080 1610 5110 5110 3480 5110 1310
1714872 800 1520 1610 5110 5110 3370 3390 1310
1715087 800 1520 2040 4990 5110 3370 3390 1310
1715303 790 1520 2040 4990 5110 3270 3390 1310
1715516 790 1310 1940 4880 5110 3270 3390 1310
1715731 790 1310 1940 4880 5110 3160 3170 1320
1715939 790 1220 1840 4780 5110 3160 3170 1320
1716156 790 1220 1840 4780 5110 3080 3060 1320
1716366 790 1130 1730 1830 5110 3080 3060 1320
1716584 730 1130 1730 1830 5110 2970 2950 1330
1716800 730 1050 1620 4540 5110 2970 2950 1330
1717008 730 1050 1620 4540 5110 2890 2840 1330
1717218 730 1550 1530 4540 5110 2890 2840 1330
1717436 760 1550 1530 4540 5110 2890 2740 1340
1717655 760 1430 1440 4310 5110 5110 2740 1340
1718071 750 1240 1390 4190 5110 5110 2540 1350
1718293 750 1240 5110 4090 5110 5110 2540 1350
1718503 750 1150 5110 4090 5110 5110 2440 1350
1718716 750 1150 5110 3970 5110 5110 2440 1350
1718928 730 1070 5110 3970 5110 5110 2350 1350
1719139 730 1070 5110 3850 5110 5110 2350 1350
1719359 720 1020 5110 3850 5110 5110 2260 1360
1719567 720 1020 5110 3730 5110 5110 2260 1360
1719787 720 980 5110 3730 5110 5110 2190 1360
1719994 720 980 5110 3620 5110 5110 2090 1360
1720215 730 5110 5110 3620 5110 5110 2090 1370
1720429 730 5110 5110 3510 5110 4850 2030 1370
1720641 740 5110 3440 3510 5110 4850 2030 1360
1720852 740 5110 3440 3390 5110 4730 5110 1360
1721063 730 5110 3300 3390 5110 4730 5110 1370
1721275 730 5110 3180 3270 5110 4610 5110 1370
1721699 670 5110 3070 3150 5110 4500 5110 1380
1721921 670 5110 3070 3030 5110 4400 5110 1390
1722339 660 5110 2940 2910 5110 4400 5110 1400
1722561 660 5010 2820 2800 5110 4400 5110 1400
1722769 660 5010 2820 2800 5110 4180 5110 1430
1722982 660 4890 2700 2680 5110 4180 5110 1430
1723203 660 4890 2700 2680 5110 4070 5110 1470
1723416 650 4780 2590 2570 5110 4070 5110 1470
1723627 650 4780 2590 2570 5110 3960 5110 1490
1723839 650 4690 2470 2570 5110 3960 5110 1490
1724051 650 4690 2470 2570 5110 3860 5110 1490
1724271 630 4600 2350 2360 5110 3730 5110 1440
1724480 630 4500 2230 2240 5110 3730 5110 1440
1724696 630 4500 2230 2240 5110 3730 5110 1420
1724905 630 4400 2110 2140 5110 3730 5110 1420
1725117 620 4400 2110 2140 5110 3550 5110 1420
1725337 620 2080 2020 5110 5110 3550 5110 1420
1725544 620 2080 2020 5110 5110 3430 5110 1430
1725755 620 4260 1910 1950 5110 3430 5110 1430
1725975 620 4260 1910 1950 5110 3340 5110 1420
1726185 620 1880 1840 1900 5110 3340 5110 1420
1726403 640 1780 1840 1900 5110 3260 5110 1420
1726612 640 1780 1840 5110 5110 3190 5110 1420
1726824 650 1700 1840 5110 5110 3190 5110 1400
1727247 690 1620 5110 5110 5110 3120 5110 1390
1727467 700 1520 1640 5110 5110 3000 5110 1370
1727680 700 1520 1560 5110 5110 2880 5110 1370
1727890 710 1440 1560 5110 5110 2880 5110 1350
1728104 710 1440 5110 5110 5110 2790 5110 1350
1728322 730 1350 5110 5110 5110 2790 5110 1330
1728531 730 1350 5110 5110 5110 2680 5110 1330
1728748 740 1290 5110 5110 5110 2680 5110 900
1728962 740 1290 5110 5110 5110 2570 3140 900
1729173 750 5110 5110 5110 5110 2570 3140 740
1729390 750 5110 5110 5110 5110 890 3030 740
1729808 780 5110 5110 5110 5110 840 2920 1290
1730032 860 5110 5110 5110 5110 840 2920 1290
1730238 860 5110 5110 5110 5110 2270 2240 1290
1730456 870 5110 5110 5110 5110 2270 2240 1270
1730664 870 5110 5110 1770 1290 5110 2120 1270
1730878 890 5110 5110 1640 4930 5110 2040 1250
1731094 890 5020 5110 2220 4930 5110 2040 1250
1731307 890 5020 5110 2220 4820 5110 1920 1240
1731521 880 3610 2800 5110 4820 5110 1920 1240
1731731 880 3610 2800 5110 4710 5110 1830 1230
1731943 850 4860 2880 5110 4710 5110 1830 1230
1732160 850 4860 2880 5110 4710 4580 1750 1220
1732378 850 4760 5110 5110 4710 4580 1750 1220
1732589 850 4760 5110 5110 5110 4460 1650 1210
1732800 870 4680 5110 5110 5110 4460 1650 1210
1733019 2850 4680 5110 5110 5110 4350 5110 1200
1733232 2850 4560 5110 5110 5110 4350 5110 1200
1733443 2860 4560 5110 5110 5110 4220 5110 1200
1733651 2860 4520 5110 5110 5110 4220 5110 1200
1734076 2880 4430 5110 5110 5110 3940 5110 1350
1734300 2880 5110 5110 5110 5110 3940 5110 1350
1734717 2880 5110 5110 5110 5110 3830 5110 2320
1734932 2890 5110 5110 5110 5110 3700 5110 2280
1735146 2890 5110 5110 5110 5110 3580 5110 2280
1735364 2890 5110 5110 5110 5110 3580 5110 2030
1735577 2890 5110 5110 5110 5110 3460 5110 2030
1735788 2890 5110 5110 5110 5110 3460 5110 1170
1736006 2890 5110 5110 5110 5110 3340 5110 1170
1736214 2900 5110 5110 5110 5110 3340 5110 1150
1736425 2900 5110 5110 5110 5110 3240 5110 1150
1736647 2900 5110 5110 5110 5110 3240 5110 1120
1736854 2900 5110 5110 5110 5110 3130 5110 1120
1737069 2910 5110 5110 5110 5110 3130 5110 1150
1737280 2900 5110 5110 5110 5110 3010 5110 1160
1737495 2900 5110 5110 5110 5110 2900 5110 1160
1737708 2910 5110 5110 5110 5110 2900 5110 1160
1737924 2910 5110 5110 5110 5110 2800 5110 1160
1738140 2900 5110 5110 5110 5110 2800 5110 1160
1738350 2900 5110 5110 5110 5110 2690 5110 1160
1738567 2910 5110 5110 5110 5110 2690 5110 1160
1738775 2910 5110 5110 5110 5110 2580 2570 1160
1738986 2900 5110 5110 5110 5110 2580 2570 1150
1739208 2900 5110 5110 5110 5110 1200 2460 1150
1739419 2910 5040 5110 5110 5110 1200 2460 1160
1739631 2910 5040 5110 5110 5110 2370 2340 1160
1739843 2910 4920 5110 5110 5110 2370 2340 1160
1740058 2910 4920 5110 5110 5110 2370 2230 1160
1740271 2900 4810 5110 5110 5110 5110 2130 1160
1740483 2900 4710 5110 5110 5110 5110 2130 1160
1740693 2900 4710 5110 5110 5110 5110 2030 1160
1740912 2890 4600 5110 5110 5110 5110 2030 1160
1741123 2890 4600 5110 5110 5110 5110 1920 1170
1741338 2900 4520 5110 5110 5110 5110 1920 1170
1741554 2900 4520 5110 5110 5110 5110 1840 1160
1741763 2900 4450 5110 5110 5110 5110 1840 1160
1741976 2920 4450 5110 5110 5110 5110 1770 1170
1742188 2920 4330 5110 5110 5110 5110 1770 1170
1742405 2970 4330 5110 5110 5110 4560 5110 1170
1742615 2970 4240 5110 5110 5110 4560 5110 1170
1742834 2970 4240 5110 5110 5110 4560 5110 1170
1743043 2970 4140 5110 5110 5110 4560 5110 1170
1743254 2950 4140 5110 5110 5110 5110 5110 1170
1743467 2950 4080 5110 5110 5110 5110 5110 1170
1743688 2910 4000 5110 5110 5110 5110 5110 1190
1743896 2890 3920 5110 5110 5110 5110 5110 1240
1744112 2890 3920 5110 5110 5110 4000 5110 1240
1744329 2880 3810 5110 5110 5110 4000 5110 1280
1744535 2880 3810 5110 5110 5110 4000 5110 1280
1744757 2940 3760 5110 5110 5110 4000 5110 1320
1744967 2940 3760 5110 5110 5110 5110 5110 1320
1745180 3040 3720 5110 5110 5110 5110 3820 1330
1745392 3040 3720 5110 5110 5110 5110 3820 1330
1745605 3020 5110 5110 5110 5110 5110 3790 5110
1745822 3020 5110 3650 5110 2420 5110 3790 5110
1746031 3010 5110 3650 5110 2420 5110 3790 5110
1746251 3010 5110 3580 5110 2340 5110 5110 5110
1746459 3000 5110 3510 5110 2340 5110 5110 5110
1746677 3000 5110 4900 3500 5110 5110 5110 5110
1746887 3000 5110 4900 3400 5110 5110 5110 5110
1747096 5110 5110 4830 3400 5110 5110 5110 1720
1747313 5110 5110 4830 3300 5110 5110 5110 1720
1747522 5110 4320 5110 3300 5110 5110 5110 1720
1747736 5110 4320 5110 3220 5110 5110 5110 1720
1747952 5110 4250 5110 3220 5110 5110 5110 5110
1748171 5110 4250 5110 3110 5110 5110 1860 5110
1748379 4940 4160 5110 4540 5110 5110 1860 3500
1748598 4940 4160 5110 4540 3010 5110 1780 3460
1748805 4950 4090 5110 4430 3010 5110 1780 3460
1749024 4950 4090 5110 4430 2870 4710 1700 3460
1749234 4950 4000 5110 4330 2870 4710 1700 3460
1749442 4950 4000 5110 4330 2760 5110 1700 3470
1749655 4960 3930 5110 4330 2760 5110 1700 3470
1749872 4960 3830 5110 5070 2660 5110 5110 3530
1750093 4960 3830 5110 5070 2550 5110 5110 3530
1750300 4960 3750 3760 4930 2550 5110 5110 3530
1750517 4980 3750 3760 4930 2450 2460 4300 5110
1750727 4980 3670 3660 4830 2330 2460 4300 5110
1750945 4990 3670 3660 4830 2330 2460 5110 5110
1751156 4990 3610 3550 4740 3730 2460 5110 5110
1751369 4990 3610 3550 4740 3730 2220 5100 5110
1751579 4990 3540 3460 5110 3610 2220 5100 5110
1751799 5000 3540 3460 5110 3610 2080 5110 5110
1752004 5000 3440 5110 5110 3500 2080 5110 5110
1752218 5010 3440 5110 5110 3500 1970 1430 5110
1752430 5010 3360 4380 5110 3370 1970 1430 5110
1752653 5020 3360 4380 5110 3370 1860 4970 2900
1752863 5020 3250 4270 5110 3250 1740 4970 2900
1753078 2470 3160 4160 5110 3120 1630 4840 1160
1753291 2430 3070 4160 5110 3120 1630 1670 1120
1753497 2430 3070 3110 5110 3010 1560 1670 1120
1753712 2410 2980 3110 5110 3010 1560 1530 2540
1753931 2410 2980 2990 5110 2900 5110 1530 2540
1754147 2390 2890 2990 5110 2900 5110 1410 1040
1754359 2390 2890 2740 5110 2800 2790 1410 1040
1754564 2380 2810 2740 5110 2800 2790 1320 2060
1754779 2380 2810 3740 5110 2700 2680 1320 2060
1754997 2350 2720 3640 5110 2700 2680 1220 980
1755208 2350 2720 3640 5110 3540 2570 1150 980
1755418 2350 2630 3540 5110 3540 2570 1150 980
1755633 2350 2630 3540 5110 3440 2450 4020 980
1755846 1340 2550 3450 5110 3440 2450 4020 1000
1756067 1340 2550 3450 5110 3250 2330 3950 1000
1756278 1320 2460 3320 5110 3130 2210 3860 1000
1756488 1050 2370 3210 5110 3130 2210 3860 960
1756702 1050 2370 3210 5110 3000 2110 3790 960
1756913 1330 2050 3110 5110 3000 2110 3790 950
1757128 1330 2050 3110 5110 2890 2010 2330 880
1757348 1000 2230 3010 5110 2890 2010 2330 880
1757559 1000 2230 3010 5110 2780 640 5110 800
1757771 1350 1860 2900 5110 2780 640 5110 800
1757985 1350 1860 2900 5110 2670 1820 1810 790
1758404 1360 1070 2810 5110 2670 2580 1680 770
1758623 1370 5110 1250 5110 2670 2580 1680 770
1759045 1380 5110 1250 5110 5110 2470 1580 760
1759260 1380 5110 1250 5110 5110 2350 1490 750
1759475 1400 1470 1600 5110 5110 2230 1430 750
1759692 1400 2200 2040 5110 5110 2230 1430 750
1759904 1420 2200 2040 5110 5110 2090 1350 990
1760117 1420 2410 2180 2310 3040 2090 1350 990
1760331 1420 2410 2180 2310 3040 1960 2050 990
1760544 1420 2180 2160 3060 2920 1960 2050 990
1760754 1430 2180 2160 3060 2920 1850 1920 980
1761179 1450 1560 5110 2970 2800 1740 1820 960
1761606 1460 1970 5110 2970 2680 1620 1710 960
1761821 1460 1860 5080 5110 2560 1500 1710 960
1762041 1470 1770 5080 5110 2560 1500 1710 950
1762253 1470 1770 4950 5110 2460 1400 1710 950
1762463 1480 1680 4840 5110 3570 1300 1510 940
1762886 910 1590 4730 5110 3440 5110 1420 930
1763108 900 1520 4730 5110 3440 5110 1310 920
1763315 900 1520 4640 5110 3470 4300 1310 920
1763530 910 1430 4640 5110 3470 4300 1080 930
1763742 910 1430 4530 5110 3470 1990 1080 930
1763960 910 4650 4440 5110 3470 1990 980 920
1764170 910 4650 4440 5110 5110 1880 980 920
1764388 910 4650 4320 5110 5110 1880 980 920
1764603 910 4650 4320 5110 5110 1770 3140 920
1764811 910 5110 4210 5110 5110 1770 3140 920
1765025 910 5110 4210 4870 5110 1680 3030 920
1765242 900 5110 4100 4870 5110 1680 3030 920
1765450 900 5110 4100 4750 5110 3280 2930 920
1765669 900 5110 3980 4630 5110 3150 2860 940
1765881 890 5110 3880 4630 5110 3150 2860 940
1766097 890 5110 3880 4520 5110 3050 1350 650
1766308 900 5110 3770 4390 5110 3050 1350 650
1766518 900 5110 3770 4390 5110 2930 1240 640
1766735 890 5110 3660 4280 5110 2930 1240 640
1766948 890 5110 3660 4280 5110 2810 1140 650
1767161 890 5110 3540 4170 5110 2810 1140 650
1767372 890 5110 3540 4170 5110 2700 1040 4160
1767582 890 5110 3440 4050 5110 2700 1040 4160
1767796 890 5110 3440 4050 5110 2670 970 3120
1768222 930 3370 3330 3930 5110 2500 2520 4230
1768443 940 3260 3230 3820 5110 2500 2520 4230
1768861 1490 3140 3130 3710 5110 2260 2210 4240
1769085 1490 2990 3020 3590 5110 2260 2210 4240
1769298 1490 2990 3020 3590 5110 2160 2020 4220
1769504 1490 2920 2940 3500 5110 2160 2020 4220
1769724 1490 2920 2940 3500 5110 2050 1880 660
1769933 1490 2820 3350 3500 5110 2050 1880 660
1770148 1490 2820 3350 3500 5110 1940 1930 640
1770361 1490 2720 3240 4220 5110 1940 1930 640
1770569 1490 2620 3240 4220 5110 1830 1930 650
1770787 1490 2620 3110 4110 5110 1730 1930 650
1771003 1490 2530 3110 4110 5110 1730 1720 640
1771213 1490 2530 3000 5110 5110 1630 1720 640
1771427 1490 2470 3000 5110 5110 1630 1720 650
1771643 1490 2470 2880 5110 5110 1530 1720 650
1771852 1500 2350 2880 5110 5110 1530 1520 1980
1772067 1500 2270 2730 5110 5110 1430 1520 1210
1772282 1500 2270 2600 5110 5110 1320 1520 1210
1772500 1510 2210 2600 5110 5110 1320 1300 1190
1772710 1510 2210 2570 5110 5110 1320 1220 1190
1772921 1510 2740 2380 5110 5110 1320 1220 1210
1773131 1510 2740 2380 5110 5110 5110 1100 1210
1773344 1510 2640 2340 5110 5110 5110 1100 1240
1773567 1510 2640 2340 5110 5110 5110 1000 1240
1773777 1510 2540 2240 5110 5110 5110 1000 1240
1773986 1510 2540 2240 5110 5110 4370 910 1240
1774197 1520 2440 2130 5110 5110 4370 910 1250
1774410 1520 2440 2130 5110 5110 4240 3590 1250
1774632 1510 2060 2030 5110 5110 4240 3590 1240
1774838 1510 2060 2030 5110 5110 4120 3500 1240
1775264 1510 1830 1820 5110 5110 3990 3410 1190
1775486 1510 1830 1820 5110 5110 3870 3390 1200
1775696 1510 1730 2410 5110 5110 3870 3390 1200
1775904 1510 1730 2410 5110 5110 3750 3250 630
1776119 1520 1620 2290 5110 5110 3750 3250 630
1776339 1520 1620 2290 5110 5110 3630 4780 620
1776545 1520 1530 2190 5110 5110 3630 4780 620
1776760 1520 1530 2190 5110 5110 3530 4700 620
1776979 1510 1430 2100 5110 5110 3530 4700 620
1777188 1510 1430 2100 5110 5110 3440 4620 620
1777405 1510 1360 2010 5110 5110 3440 4620 620
1777618 1500 1360 2010 5110 5110 3370 4570 640
1777829 1500 1280 1930 3820 5110 3370 4570 640
1778252 1400 1210 1830 3710 5110 3380 3170 680
1778473 1400 1120 420 3610 5110 3380 3170 680
1778892 1390 1060 1670 2570 5110 3290 3090 1280
1779109 1380 1060 1670 2570 5110 3250 3050 5110
1779324 1380 1040 1650 2550 5110 3240 3050 5110
1686487 0 0 0
1686915 0 0 0
1687132 0 0 0
1687555 0 0 0
1687770 0 0 0
1688194 0 0 0
1688411 0 0 0
1688627 0 0 0
1688843 0 0 0
1689054 0 0 0
1689273 0 0 0
1689479 0 0 0
1689698 0 0 0
1689906 0 0 0
1690116 0 0 0
1690338 0 0 0
1690549 0 0 0
1690759 0 0 0
1690970 0 0 0
1691192 0 0 0
1691400 0 0 0
1691822 0 0 0
1692039 0 0 0
1692258 0 0 0
1692470 0 0 0
1692681 0 0 0
1692900 0 0 0
1693105 0 0 0
1693319 0 0 0
1693530 0 0 0
1693751 0 0 0
1693959 0 0 0
1694178 0 0 0
1694387 0 0 0
1694811 0 0 0
1695028 0 0 0
1695243 0 0 0
1695451 0 0 0
1695665 0 0 0
1695888 0 0 0
1696098 0 0 0
1696314 0 0 0
1696524 0 0 0
1696740 0 0 0
1696952 0 0 0
1697168 0 0 0
1697377 0 0 0
1697590 10 0 3600
1697802 33 0 0
1698013 71 0 0
1698233 122 0 1
1698442 184 0 0
1698652 249 0 0
1698868 379 0 0
1699087 445 0 0
1699293 511 0 3600
1699515 574 0 3599
1699721 636 0 3599
1699939 701 0 3597
1700150 767 0 3596
1700362 829 0 3595
1700578 888 -1 3595
1700788 951 -1 3594
1701000 1015 -2 3594
1701218 1077 -2 3594
1701435 1142 -3 3593
1701645 1208 -4 3592
1701857 1274 -5 3592
1702070 1340 -6 3592
1702289 1470 -7 3591
1702500 1530 -8 3591
1702713 1581 -10 3570
1702928 1624 -13 3549
1703144 1675 -18 3547
1703349 1733 -23 3549
1703566 1792 -28 3550
1703775 1855 -34 3551
1703990 1918 -39 3553
1704210 1981 -44 3555
1704423 2039 -49 3556
1704637 2103 -54 3557
1704845 2167 -58 3558
1705065 2223 -62 3562
1705272 2283 -66 3563
1705488 2411 -74 3567
1705695 2477 -78 3569
1705912 2540 -81 3570
1706122 2601 -84 3572
1706339 2668 -87 3574
1706549 2731 -90 3577
1706767 2792 -92 3578
1706977 2857 -95 3580
1707189 2919 -97 3582
1707409 2982 -99 3584
1707616 3039 -100 3586
1707834 3097 -102 3585
1708047 3161 -103 3585
1708261 3216 -105 3582
1708478 3338 -109 3581
1708688 3401 -111 3581
1708902 3468 -113 3580
1709118 3531 -115 3581
1709329 3595 -117 3583
1709540 3657 -119 3584
1709759 3718 -121 3585
1709968 3781 -122 3586
1710181 3845 -124 3588
1710391 3906 -125 3590
1710611 3970 -126 3592
1710820 4029 -127 3592
1711032 4080 -127 3592
1711247 4136 -128 3592
1711464 4202 -129 3591
1711882 4388 -132 3590
1712100 4451 -134 3589
1712317 4514 -135 3588
1712525 4579 -136 3587
1712739 4646 -138 3588
1712952 4711 -139 3589
1713170 4772 -140 3591
1713385 4836 -141 3592
1713599 4900 -142 3592
1713813 4957 -143 3593
1714021 5010 -143 3592
1714235 5068 -144 3591
1714452 5133 -145 3590
1714657 5199 -147 3589
1714872 5329 -150 3586
1715087 5395 -151 3585
1715303 5460 -153 3584
1715516 5518 -155 3583
1715731 5576 -156 3582
1715939 5639 -159 3581
1716156 5704 -161 3580
1716366 5768 -163 3579
1716584 5831 -165 3579
1716800 5894 -167 3581
1717008 5957 -170 3582
1717218 6022 -172 3583
1717436 6081 -173 3583
1717655 6143 -175 3585
1718071 6322 -180 3587
1718293 6383 -181 3589
1718503 6444 -182 3589
1718716 6507 -183 3590
1718928 6571 -184 3592
1719139 6637 -185 3592
1719359 6699 -186 3593
1719567 6760 -187 3594
1719787 6825 -187 3594
1719994 6890 -188 3593
1720215 6953 -189 3592
1720429 7013 -190 3591
1720641 7077 -191 3591
1720852 7143 -192 3590
1721063 7206 -193 3589
1721275 7336 -196 3587
1721699 7462 -199 3586
1721921 7528 -200 3585
1722339 7658 -204 3584
1722561 7721 -206 3585
1722769 7785 -207 3586
1722982 7849 -209 3586
1723203 7914 -210 3586
1723416 7978 -212 3585
1723627 8042 -214 3585
1723839 8104 -215 3584
1724051 8168 -217 3583
1724271 8297 -221 3581
1724480 8362 -224 3580
1724696 8425 -226 3580
1724905 8485 -228 3584
1725117 8540 -229 3591
1725337 8595 -230 0
1725544 8648 -229 12
1725755 8697 -227 28
1725975 8740 -224 42
1726185 8783 -221 56
1726403 8832 -215 70
1726612 8875 -209 87
1726824 8920 -202 103
1727247 9026 -180 118
1727467 9148 -155 112
1727680 9208 -143 106
1727890 9268 -132 101
1728104 9328 -122 94
1728322 9391 -111 89
1728531 9455 -102 82
1728748 9517 -93 77
1728962 9578 -85 75
1729173 9637 -77 72
1729390 9700 -69 72
1729808 9821 -54 71
1730032 9887 -45 69
1730238 9952 -37 68
1730456 10018 -29 68
1730664 10083 -22 66
1730878 10215 -7 61
1731094 10278 0 58
1731307 10341 6 57
1731521 10407 12 56
1731731 10473 19 55
1731943 10539 25 54
1732160 10605 31 53
1732378 10671 38 51
1732589 10738 44 51
1732800 10804 50 50
1733019 10870 55 50
1733232 10936 61 49
1733443 11002 67 48
1733651 11068 73 47
1734076 11264 88 43
1734300 11329 94 43
1734717 11461 103 41
1734932 11527 108 41
1735146 11593 113 39
1735364 11658 118 38
1735577 11724 122 36
1735788 11790 126 36
1736006 11854 130 34
1736214 11920 134 33
1736425 11986 138 32
1736647 12052 142 31
1736854 12118 145 30
1737069 12184 149 29
1737280 12315 156 28
1737495 12381 159 27
1737708 12447 162 26
1737924 12514 165 25
1738140 12580 168 25
1738350 12646 171 24
1738567 12712 174 24
1738775 12777 177 23
1738986 12843 180 22
1739208 12909 182 21
1739419 12974 185 21
1739631 13040 187 21
1739843 13105 190 20
1740058 13170 192 19
1740271 13299 196 17
1740483 13364 198 16
1740693 13429 200 17
1740912 13493 202 16
1741123 13559 204 15
1741338 13624 206 14
1741554 13689 208 14
1741763 13756 209 14
1741976 13822 211 13
1742188 13887 213 12
1742405 13953 214 11
1742615 14019 215 11
1742834 14084 217 10
1743043 14150 218 9
1743254 14216 219 8
1743467 14281 220 9
1743688 14408 223 12
1743896 14468 224 0
1744112 14516 223 3572
1744329 14556 220 3542
1744535 14593 215 3512
1744757 14631 208 3481
1744967 14668 199 3450
1745180 14707 187 3422
1745392 14750 173 3396
1745605 14789 157 3368
1745822 14827 139 3344
1746031 14867 119 3321
1746251 14906 97 3298
1746459 14946 73 3275
1746677 15023 20 3235
1746887 15062 -9 3217
1747096 15099 -40 3198
1747313 15139 -75 3181
1747522 15180 -112 3165
1747736 15220 -152 3150
1747952 15259 -191 3136
1748171 15298 -233 3123
1748379 15334 -274 3109
1748598 15366 -312 3097
1748805 15401 -354 3083
1749024 15436 -401 3072
1749234 15472 -448 3060
1749442 15503 -493 3048
1749655 15536 -542 3038
1749872 15599 -639 3018
1750093 15632 -694 3008
1750300 15662 -746 2998
1750517 15693 -800 2990
1750727 15723 -855 2981
1750945 15751 -909 2974
1751156 15779 -963 2967
1751369 15805 -1016 2960
1751579 15831 -1071 2954
1751799 15857 -1127 2948
1752004 15883 -1183 2942
1752218 15908 -1240 2938
1752430 15935 -1301 2938
1752653 15961 -1361 2932
1752863 15987 -1422 2927
1753078 16036 -1542 2918
1753291 16058 -1599 2913
1753497 16080 -1656 2908
1753712 16102 -1715 2902
1753931 16123 -1773 2897
1754147 16143 -1830 2893
1754359 16163 -1888 2887
1754564 16182 -1945 2883
1754779 16201 -2004 2878
1754997 16219 -2062 2872
1755208 16237 -2119 2868
1755418 16254 -2178 2863
1755633 16271 -2238 2858
1755846 16288 -2298 2853
1756067 16304 -2358 2850
1756278 16336 -2478 2843
1756488 16351 -2539 2840
1756702 16366 -2600 2835
1756913 16380 -2660 2830
1757128 16393 -2719 2826
1757348 16406 -2779 2821
1757559 16419 -2841 2814
1757771 16431 -2902 2810
1757985 16443 -2965 2805
1758404 16463 -3087 2792
1758623 16472 -3148 2785
1759045 16490 -3272 2776
1759260 16497 -3332 2775
1759475 16514 -3460 2777
1759692 16523 -3526 2778
1759904 16532 -3591 2778
1760117 16540 -3654 2779
1760331 16549 -3716 2780
1760544 16558 -3780 2783
1760754 16567 -3844 2785
1761179 16586 -3969 2788
1761606 16606 -4098 2792
1761821 16616 -4162 2793
1762041 16626 -4224 2789
1762253 16634 -4281 2783
1762463 16652 -4408 2785
1762886 16669 -4527 2771
1763108 16675 -4586 2764
1763315 16682 -4647 2755
1763530 16687 -4708 2749
1763742 16692 -4769 2742
1763960 16696 -4830 2736
1764170 16699 -4889 2727
1764388 16701 -4948 2720
1764603 16703 -5011 2718
1764811 16705 -5076 2716
1765025 16706 -5139 2713
1765242 16707 -5202 2714
1765450 16709 -5267 2716
1765669 16712 -5391 2719
1765881 16714 -5455 2720
1766097 16717 -5516 2721
1766308 16719 -5581 2723
1766518 16722 -5645 2725
1766735 16724 -5707 2727
1766948 16727 -5772 2727
1767161 16730 -5839 2726
1767372 16733 -5903 2726
1767582 16735 -5964 2727
1767796 16738 -6029 2726
1768222 16744 -6159 2728
1768443 16747 -6223 2730
1768861 16756 -6419 2730
1769085 16759 -6483 2730
1769298 16763 -6549 2729
1769504 16766 -6613 2730
1769724 16769 -6679 2730
1769933 16772 -6745 2730
1770148 16775 -6809 2730
1770361 16779 -6876 2729
1770569 16782 -6941 2729
1770787 16785 -7005 2731
1771003 16788 -7065 2734
1771213 16791 -7123 2738
1771427 16795 -7185 2739
1771643 16800 -7248 2740
1771852 16804 -7310 2741
1772067 16813 -7442 2741
1772282 16817 -7504 2740
1772500 16821 -7567 2741
1772710 16826 -7632 2739
1772921 16830 -7696 2737
1773131 16834 -7760 2735
1773344 16838 -7827 2734
1773567 16842 -7893 2734
1773777 16845 -7959 2734
1773986 16849 -8025 2733
1774197 16853 -8091 2732
1774410 16856 -8157 2732
1774632 16860 -8222 2731
1774838 16863 -8288 2730
1775264 16873 -8482 2728
1775486 16876 -8547 2727
1775696 16879 -8613 2727
1775904 16882 -8678 2727
1776119 16884 -8742 2725
1776339 16886 -8802 2720
1776545 16888 -8859 2713
1776760 16889 -8916 2705
1776979 16889 -8968 2695
1777188 16888 -9018 2683
1777405 16885 -9068 2667
1777618 16881 -9117 2649
1777829 16876 -9167 2632
1778252 16854 -9340 2630
1778473 16849 -9384 2630
1778892 16842 -9433 2630
1779109 16842 -9437 2631
1779324 16842 -9437 2631
#include <iostream>
#include <math.h>
#include <vector>
using namespace std;

// Sensor characteristic: Min and Max ranges of the beams
double Zmax = 5000, Zmin = 170;
// Defining free cells(lfree), occupied cells(locc), unknown cells(l0) log odds values
double l0 = 0, locc = 0.4, lfree = -0.4;
// Grid dimensions
double gridWidth = 100, gridHeight = 100;
// Map dimensions
double mapWidth = 30000, mapHeight = 15000;
// Robot size with respect to the map 
double robotXOffset = mapWidth / 5, robotYOffset = mapHeight / 3;
// Defining an l vector to store the log odds values of each cell
vector< vector<double> > l(mapWidth/gridWidth, vector<double>(mapHeight/gridHeight));

double inverseSensorModel(double x, double y, double theta, double xi, double yi, double sensorData[])
{
    // You will be coding this section in the upcoming concept! 
    return 0.4;
}

void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[])
{
    //******************Code the Occupancy Grid Mapping Algorithm**********************//
    for (int x = 0; x < mapWidth / gridWidth; x++) {
        for (int y = 0; y < mapHeight / gridHeight; y++) {
            double xi = x * gridWidth + gridWidth / 2 - robotXOffset;
            double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset;
            if (sqrt(pow(xi - Robotx, 2) + pow(yi - Roboty, 2)) <= Zmax) {
                l[x][y] = l[x][y] + inverseSensorModel(Robotx, Roboty, Robottheta, xi, yi, sensorData) - l0;
            }
        }
    }
}

int main()
{
    double timeStamp;
    double measurementData[8];
    double robotX, robotY, robotTheta;

    FILE* posesFile = fopen("poses.txt", "r");
    FILE* measurementFile = fopen("measurement.txt", "r");

    // Scanning the files and retrieving measurement and poses at each timestamp
    while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) {
        fscanf(measurementFile, "%lf", &timeStamp);
        for (int i = 0; i < 8; i++) {
            fscanf(measurementFile, "%lf", &measurementData[i]);
        }
        occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData);
    }
    
    // Displaying the map
    for (int x = 0; x < mapWidth / gridWidth; x++) {
        for (int y = 0; y < mapHeight / gridHeight; y++) {
            cout << l[x][y] << " ";
        }
    }
    
    return 0;
}